#pragma config(Sensor, in1,    ArmPot,         sensorPotentiometer)
#pragma config(Sensor, in2,    Gyro,           sensorGyro)
#pragma config(Sensor, dgtl1,  solenoid,       sensorDigitalOut)
#pragma config(Sensor, dgtl2,  GripperLeft,    sensorDigitalOut)
#pragma config(Sensor, dgtl3,  GripperRight,   sensorDigitalOut)
#pragma config(Motor,  port1,           RightBackBase, tmotorVex393HighSpeed_HBridge, openLoop, reversed)
#pragma config(Motor,  port2,           RightTurntable, tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port3,           LeftTurntable, tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           LeftBackBase,  tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port5,           LeftFrontBase, tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port6,           LeftArm,       tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port7,           RightArm,      tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           LeftIntake,    tmotorVex269_MC29, openLoop)
#pragma config(Motor,  port9,           RightIntake,   tmotorVex269_MC29, openLoop, reversed)
#pragma config(Motor,  port10,          RightFrontBase, tmotorVex393HighSpeed_HBridge, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

#define ground 0
#define goal 800
#define motorhold 10

void WaitForLCD()
{

	while(true)
	{
		// we will be in this loop until the user pushes a button
		// check if button has been pressed if so break out of loop
		if (nLCDButtons != 0)
		{
			// somebody pressed a button - break out of this loop and continue on...
			break;
		}

	}
}

//Wait for Press--------------------------------------------------
void waitForPress()
{
	while(nLCDButtons == 0){}
	wait1Msec(5);
}
//----------------------------------------------------------------

task BatmanSong()
{
	//        400 = Tempo
	//          5 = Default octave
	//     Eighth = Default note length
	//        10% = Break between notes
	//
	playTone(  587,   14); wait1Msec( 150);  // Note(D)
	playTone(  587,   14); wait1Msec( 150);  // Note(D)
	playTone(  554,   14); wait1Msec( 150);  // Note(C#)
	playTone(  554,   14); wait1Msec( 150);  // Note(C#)
	playTone(  523,   14); wait1Msec( 150);  // Note(C)
	playTone(  523,   14); wait1Msec( 150);  // Note(C)
	playTone(  554,   14); wait1Msec( 150);  // Note(C#)
	playTone(  554,   14); wait1Msec( 150);  // Note(C#)
	playTone(  587,   14); wait1Msec( 150);  // Note(D)
	playTone(  587,   14); wait1Msec( 150);  // Note(D)
	playTone(  554,   14); wait1Msec( 150);  // Note(C#)
	playTone(  554,   14); wait1Msec( 150);  // Note(C#)
	playTone(  523,   14); wait1Msec( 150);  // Note(C)
	playTone(  523,   14); wait1Msec( 150);  // Note(C)
	playTone(  554,   14); wait1Msec( 150);  // Note(C#)
	playTone(  554,   14); wait1Msec( 150);  // Note(C#)
	playTone(  587,   14); wait1Msec( 150);  // Note(D)
	playTone(  622,   14); wait1Msec( 150);  // Note(D#)
	playTone(  523,   14); wait1Msec( 150);  // Note(C)
	playTone(  554,   14); wait1Msec( 150);  // Note(C#)
	playTone(  523,   14); wait1Msec( 150);  // Note(C)
	playTone(  523,   14); wait1Msec( 150);  // Note(C)
	playTone(  554,   14); wait1Msec( 150);  // Note(C#)
	playTone(  554,   14); wait1Msec( 150);  // Note(C#)
	playTone(  698,   14); wait1Msec( 150);  // Note(F)
	playTone(    0,   14); wait1Msec( 150);  // Note(Rest)
	playTone(  698,   27); wait1Msec( 300);  // Note(F, Duration(Quarter))
}

void pre_auton()
{
	bStopTasksBetweenModes = true;
	SensorValue[solenoid] = 0;  // ...deactivate the solenoid.
}

void AArms(int Height, int Speed)
{
	motor(LeftArm) = Speed;
	motor(RightArm) = Speed;
	wait1Msec(Height);
	motor(LeftArm) = 15;
	motor(RightArm) = 15;
	//bool GoingUp = false;
	////detect if going up or down
	//if(Height < SensorValue(ArmPot))
	//{
	//	GoingUp = true;
	//}
	//else
	//{
	//	GoingUp = false;
	//}

	//if(GoingUp == true)
	//{
	//	while(Height < SensorValue(ArmPot))
	//	{
	//		motor(RightArm) = Speed;
	//		motor(LeftArm) = Speed;
	//	}
	//	motor(RightArm) = motorhold;
	//	motor(LeftArm) = motorhold;
	//}

	//if(GoingUp == false)
	//{
	//	while(Height > SensorValue(ArmPot))
	//	{
	//		motor(RightArm) = -abs(Speed);
	//		motor(LeftArm) = -abs(Speed);
	//	}
	//	motor(RightArm) = motorhold;
	//	motor(LeftArm) = motorhold;
	//}
}

void ASideways(bool direction, int distance) //   true = left   false = right    distance is in time (msec) ATM
{
	if(direction == false) // to the left
	{
		motor(LeftBackBase) = -127/1.5;
		motor(LeftFrontBase) = 127/1.5;
		motor(RightBackBase) = 127/1.5;
		motor(RightFrontBase) = -127/1.5;
		wait1Msec(distance);
	}

	motor(LeftBackBase) = 0;
	motor(LeftFrontBase) = 0;
	motor(RightBackBase) = 0;
	motor(RightFrontBase) = 0;

	if(direction == true) // to the right
	{
		motor(LeftBackBase) = 127/1.5;
		motor(LeftFrontBase) = -127/1.5;
		motor(RightBackBase) = -127/1.5;
		motor(RightFrontBase) = 127/1.5;
		wait1Msec(distance);
	}

	motor(LeftBackBase) = 0;
	motor(LeftFrontBase) = 0;
	motor(RightBackBase) = 0;
	motor(RightFrontBase) = 0;
}

void ATurn(int degree, bool clockwise) // degree in time ATM
{
	if(clockwise == true)
	{
		motor(LeftBackBase) = 127;
		motor(LeftFrontBase) = 127;
		motor(RightBackBase) = -127;
		motor(RightFrontBase) = -127;
		wait1Msec(degree);
	}

	motor(LeftBackBase) = 0;
	motor(LeftFrontBase) = 0;
	motor(RightBackBase) = 0;
	motor(RightFrontBase) = 0;

	if(clockwise == false)
	{
		motor(LeftBackBase) = -127;
		motor(LeftFrontBase) = -127;
		motor(RightBackBase) = 127;
		motor(RightFrontBase) = 127;
		wait1Msec(degree);
	}

	motor(LeftBackBase) = 0;
	motor(LeftFrontBase) = 0;
	motor(RightBackBase) = 0;
	motor(RightFrontBase) = 0;
}

void AMove(bool Forward, int distance, int speed) // Forward = true, Backward = false, Distance is in time (msec) ATM
{
	if(Forward == true)
	{
		motor(LeftBackBase) = abs(speed);
		motor(RightBackBase) = abs(speed);
		motor(LeftFrontBase) = abs(speed);
		motor(RightFrontBase) = abs(speed);
		wait1Msec(distance);
	}

	motor(LeftBackBase) = 0;
	motor(LeftFrontBase) = 0;
	motor(RightBackBase) = 0;
	motor(RightFrontBase) = 0;

	if(Forward == false)
	{
		motor(LeftBackBase) = -abs(speed);
		motor(RightBackBase) = -abs(speed);
		motor(LeftFrontBase) = -abs(speed);
		motor(RightFrontBase) = -abs(speed);
		wait1Msec(distance);
	}

	motor(LeftBackBase) = 0;
	motor(LeftFrontBase) = 0;
	motor(RightBackBase) = 0;
	motor(RightFrontBase) = 0;

}

void AKicker()
{
	SensorValue[solenoid] = 1;  // ...activate the solenoid.
}

void ABlueLeft()
{
	AKicker();
	AMove(true, 800, 100);
	ATurn(500, false);
	AMove(false, 1600, 100);
}

void ARedLeft()
{
	AArms(goal, 63);	// extend arms
	AMove(true, 1100, 100); 	// drive forward knock off ball
	AMove(false, 1000, 70);	// back up
	wait1Msec(200);
	WaitForLCD();  //wait for adjustment
	AMove(true, 1400, 100); 	// drive forward
	ATurn(600, false); // go turn
	AMove(true, 1200, 127);	// knock off ball
	wait1Msec(200);
	ATurn(300, true);
	AMove(false, 1300, 127);
	WaitForLCD();
	AMove(true, 2400, 100);
	Aturn(500, false);
	AMove(true, 1000, 100);
	ATurn(300, false);
	AMove(false, 2500, 100);


	//AMove(true, 3500, 127);
	//ATurn(200, false);
	//AMove(false, 3600, 127);
	//WaitForLCD();
	//AMove(true, 4500, 127);


	//AArms(goal, 63);	// extend arms
	//AMove(true, 1200, 127/1.5); 	// drive forward knock off ball
	//AMove(false, 600, 70);	// back up
	//wait1Msec(200);
	//ASideways(true, 1300);
	//AMove(true, 700, 127/1.5);	// knock off ball
	//wait1Msec(200);
	//AMove(false, 600, 127/1.5);
	//ASideways(true, 1200);
}

void ABlueRight()
{
	AArms(goal, 63);	// extend arms
	AMove(true, 1300, 100); 	// drive forward knock off ball
	AMove(false, 1200, 70);	// back up
	wait1Msec(200);
	WaitForLCD();  //wait for adjustment
	AMove(true, 1400, 100); 	// drive forward
	ATurn(500, true); // go turn
	AMove(true, 1300, 127);	// knock off ball

	//AArms(goal, 63);	// extend arms
	//AMove(true, 1200, 100); 	// drive forward knock off ball
	//AMove(false, 600, 70);	// back up
	//wait1Msec(200);
	//ASideways(false, 1000);
	//AMove(true, 700, 127);	// knock off ball
	//wait1Msec(200);
	//AMove(false, 600, 127);
	//ASideways(false, 1200);
}

void ARedRight()
{
	AKicker();
	AMove(true, 800, 100);
	ATurn(500, true);
	AMove(false, 1600, 100);
}

task autonomous()
{
	displayLCDCenteredString(0, "Blue or Red");
	displayLCDCenteredString(1, "Blue	    		Red");
	waitForPress();
	bool Red = false;
	bool Blue = false;
	if(nLCDButtons == 1) //left or blue...
	{
		Blue = true;
	}
	if(nLCDButtons == 4) //right or red...
	{
		Red = true;
	}
	wait1Msec(500);
	clearLCDLine(0);
	clearLCDLine(1);

	displayLCDCenteredString(0, "Left or Right");
	displayLCDCenteredString(1, "Left	    		Right");
	waitForPress();
	bool Left = false;
	bool Right = false;
	if(nLCDButtons == 1)//left or blue...
	{
		Left = true;
	}
	if(nLCDButtons == 4) //right or red...
	{
		Right = true;
	}

	clearLCDLine(1);
	if(Left == true)
	{
		if(Blue == true)
		{
			displayLCDCenteredString(0, "Left Blue!");
			ABlueLeft();
		}
		if(Red == true)
		{
			displayLCDCenteredString(0, "Left Red!");
			ARedLeft();
		}
	}
	if(Right == true)
	{
		if(Blue == true)
		{
			displayLCDCenteredString(0, "Right Blue!");
			ABlueRight();
		}
		if(Red == true)
		{
			displayLCDCenteredString(0, "Right Red!");
			ARedRight();
		}
	}

}



//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~//

void UKicker()
{
	{
		if(vexRT[Btn5U] == 1)         // If button 6U (upper right shoulder button) is pressed:
		{
			SensorValue[solenoid] = 0;  // ...activate the solenoid.
		}
		else                          // If button 6U (upper right shoulder button) is  NOT pressed:
		{
			SensorValue[solenoid] = 1;  // ..deactivate the solenoid.
		}
	}
}

void UGripper()
{
	{
		if(vexRT[Btn5UXmtr2] == 1)
		{
			SensorValue[GripperLeft] = 0;
			SensorValue[GripperRight] = 0;
		}
		if(vexRT[Btn5DXmtr2] == 1)
		{
			SensorValue[GripperLeft] = 1;
			SensorValue[GripperRight] = 1;
		}
	}
}

void UBase()
{
	if(vexRT(Btn6U) == 1)
	{
		motor[RightFrontBase] = vexRT(Ch2);
		motor[RightBackBase] =  vexRT(Ch2);
		motor[LeftFrontBase] = vexRT(Ch3);
		motor[LeftBackBase] =  vexRT(Ch3);
	}
	else
	{
		motor[RightFrontBase] = (vexRT(Ch2))/2;
		motor[RightBackBase] =  (vexRT(Ch2))/2;
		motor[LeftFrontBase] = (vexRT(Ch3))/2;
		motor[LeftBackBase] =  (vexRT(Ch3))/2;
	}

	if(vexRT(Btn5D) == 1) // to the right
	{
		motor(LeftBackBase) = -127;
		motor(LeftFrontBase) = 127;
		motor(RightBackBase) = 127;
		motor(RightFrontBase) = -127;
	}
	if(vexRT(Btn6D) == 1) // to the left
	{
		motor(LeftBackBase) = 127;
		motor(LeftFrontBase) = -127;
		motor(RightBackBase) = -127;
		motor(RightFrontBase) = 127;
	}

}

void UArms()
{
	motor[RightArm] = vexRT[Ch2Xmtr2]/1.5 + 10;
	motor[LeftArm] = vexRT[Ch2Xmtr2]/1.5 + 10;
}

void UIntake()
{
	if(vexRT(Btn6UXmtr2) == 1)
	{
		motor[RightIntake] = 127;
		motor[LeftIntake] = 127;
	}
	if(vexRT(Btn6DXmtr2) == 1)
	{
		motor[RightIntake] = -127;
		motor[LeftIntake] = -127;
	}
	if(vexRT(Btn6UXmtr2) == 0 && vexRT(Btn6DXmtr2) == 0)
	{
		motor[RightIntake] = 0;
		motor[LeftIntake] = 0;
	}
}

void UTurntables()
{
	motor[RightTurntable] = vexRT(Ch3Xmtr2);
	motor[LeftTurntable] = vexRT(Ch3Xmtr2);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~//

task usercontrol()
{
	startTask(BatmanSong);
	bLCDBacklight = true; // have LCD backlight on
	string mainBattery, backupBattery;
	wait1Msec(2000);
	while(1)
	{
		// Displays the battery level on the LCD
		clearLCDLine(0);                                            // Clear line 1 (0) of the LCD
		clearLCDLine(1);                                            // Clear line 2 (1) of the LCD

		//Display the Primary Robot battery voltage
		displayLCDString(0, 0, "Primary: ");
		sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V'); //Build the value to be displayed
		displayNextLCDString(mainBattery);

		//Display the Backup battery voltage
		displayLCDString(1, 0, "Backup: ");
		sprintf(backupBattery, "%1.2f%c", BackupBatteryLevel/1000.0, 'V');    //Build the value to be displayed
		displayNextLCDString(backupBattery);

		//Short delay for the LCD refresh rate
		wait1Msec(100);

		UGripper();
		UBase();
		UArms();
		UIntake();
		UTurntables();
		UKicker();
	}
}
